﻿using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Linq;
using System.Diagnostics;

using Debug = UnityEngine.Debug;

public class Pair<T>
{
    public T first;
    public T second;

    public Pair(T first, T second)
    {
        this.first = first;
        this.second = second;
    }
}

public class Tripel<T>
{
    public T first;
    public T second;
    public T third;

    public Tripel(T first, T second, T third)
    {
        this.first = first;
        this.second = second;
        this.third = third;
    }
}

public struct Point2
{
    #region Struct stuff
    public static explicit operator Point2(Vector2 vector2)
    {
        return new Point2(vector2);
    }
    public static implicit operator Vector2(Point2 point2)
    {
        return new Vector2(point2.x, point2.y);
    }

    public static Point2 operator -(Point2 a)
    {
        return new Point2(-a.x, -a.y);
    }
    public static Point2 operator +(Point2 a, Point2 b)
    {
        return new Point2(a.x + b.x, a.y + b.y);
    }
    public static Point2 operator -(Point2 a, Point2 b)
    {
        return a + -b;
    }
    public static Point2 operator %(Point2 a, int b)
    {
        return new Point2(a.x % b, a.y % b);
    }
    public static bool operator ==(Point2 a, Point2 b)
    {
        return a.x == b.x && a.y == b.y;
    }
    public static bool operator !=(Point2 a, Point2 b)
    {
        return !(a == b);
    }

    public override string ToString()
    {
        return string.Format("Point2({0}, {1})", x, y);
    }
    public override bool Equals(object obj)
    {
        Point2 p = (Point2)obj;

        //Debug.Log("called equals");
        return x.Equals(p.x) && y.Equals(p.y);
    }

    public override int GetHashCode()
    {
        unchecked // Overflow is fine, just wrap
        {
            int hash = 13;
            hash = hash * 7 + x.GetHashCode();
            hash = hash * 7 + y.GetHashCode();
            return hash;
        }
    }
    #endregion

    public int x;
    public int y;

    public Point2(int x, int y)
    {
        this.x = x;
        this.y = y;
    }

    public Point2(float x, float y)
    {
        this.x = (int)x;
        this.y = (int)y;
    }

    public Point2(Vector2 vector2)
    {
        x = (int)vector2.x;
        y = (int)vector2.y;
    }

    public static Point2 WorldToRegion(Point2 a)
    {
        return new Point2(a.x >> 9, a.y >> 9);
    }

    public static Point2 RoundToPoint(Vector2 vector2)
    {
        return new Point2(Mathf.RoundToInt(vector2.x), Mathf.RoundToInt(vector2.y));
    }
    public static Point2 RoundToPoint(float x, float y)
    {
        return new Point2(Mathf.RoundToInt(x), Mathf.RoundToInt(y));
    }

    public static float Distance(Point2 a, Point2 b)
    {
        int deltaX = a.x - b.x;
        int deltaY = a.y - b.y;
        return Mathf.Sqrt(deltaX * deltaX + deltaY * deltaY);
    }
    public static float SqrDistance(Point2 a, Point2 b)
    {
        int deltaX = a.x - b.x;
        int deltaY = a.y - b.y;
        return deltaX * deltaX + deltaY * deltaY;

    }

}

[System.Serializable]
public struct Point3
{
    #region Struct stuff
    public static explicit operator Point3(Vector3 vector3)
    {
        return new Point3(vector3);
    }
    public static implicit operator Vector3(Point3 point3)
    {
        return new Vector3(point3.x, point3.y, point3.z);
    }

    public static Point3 operator -(Point3 a)
    {
        return new Point3(-a.x, -a.y, -a.z);
    }
    public static Point3 operator +(Point3 a, Point3 b)
    {
        return new Point3(a.x + b.x, a.y + b.y, a.z + b.z);
    }
    public static Point3 operator -(Point3 a, Point3 b)
    {
        return a + -b;
    }
    public static Point3 operator %(Point3 a, int b)
    {
        return new Point3(a.x % b, a.y % b, a.z % b);
    }
    public static bool operator ==(Point3 a, Point3 b)
    {
        return a.x == b.x && a.y == b.y && a.z == b.z;
    }
    public static bool operator !=(Point3 a, Point3 b)
    {
        return !(a == b);
    }
    public override bool Equals(object obj)
    {
        Point3 b = (Point3)obj;
        return x.Equals(b.x) && y.Equals(b.y) && z.Equals(b.z);
    }
    public override int GetHashCode()
    {
        unchecked // Overflow is fine, just wrap
        {
            int hash = 7;
            hash = hash * 5 + x;
            hash = hash * 5 + y;
            hash = hash * 5 + z;
            return hash;
        }
    }
    public override string ToString()
    {
        return string.Format("Point3({0}, {1}, {2})", x, y, z);
    }
    #endregion

    public int x;
    public int y;
    public int z;
    public static Point3 Round(Vector3 vector3)
    {
        return new Point3(Mathf.RoundToInt(vector3.x), Mathf.RoundToInt(vector3.y), Mathf.RoundToInt(vector3.z));
    }
    public static float Distance(Point3 a, Point3 b)
    {
        int deltaX = a.x - b.x;
        int deltaY = a.y - b.y;
        int deltaZ = a.z - b.z;
        return Mathf.Sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ);
    }
    public static float SqrDistance(Point3 a, Point3 b)
    {
        int deltaX = a.x - b.x;
        int deltaY = a.y - b.y;
        int deltaZ = a.z - b.z;
        return deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ;

    }

    public static Point2 ConvertToRegionPoint(Point3 point)
    {
        return new Point2(point.x >> 9, point.z >> 9);
    }
    public static Point3 ConvertToChunkPoint(Point3 point)
    {
        return new Point3(point.x >> 4, point.y >> 4, point.z >> 4);
    }

    public Point3(int x, int y, int z)
    {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    public Point3(Vector3 vector3)
    {
        x = (int)vector3.x;
        y = (int)vector3.y;
        z = (int)vector3.z;
    }

    public float magnitude { get { return Mathf.Sqrt(x * x + y * y + z * z); } }
    public int sqrMagnitude { get { return x * x + y * y + z * z; } }
}

/// <summary>
/// A min binary heap where the min object is the root
/// </summary>
/// <typeparam name="TValue">The type of the objects in the queue</typeparam>
public class PriorityQueue<TKey, TValue> where TKey : IComparable
{
    private List<TValue> Values; // The nodes
    private List<TKey> Keys;     // The f values of the nodes
    private Dictionary<TValue, int> HeapIndices;

    public PriorityQueue()
    {
        Values = new List<TValue>();
        Keys = new List<TKey>();
        HeapIndices = new Dictionary<TValue, int>();
    }

    public void Insert(TKey newKey, TValue newValue)
    {
        HeapIndices.Add(newValue, Keys.Count);
        Values.Add(newValue);
        Keys.Add(newKey);

        BubbleUp(Keys.Count - 1);
    }
    public KeyValuePair<TKey, TValue> RemoveMin()
    {
        if (Keys.Count == 0)
            throw new InvalidOperationException("Cannot extract minimum from empty heap.");

        KeyValuePair<TKey, TValue> result = new KeyValuePair<TKey, TValue>(Keys[0], Values[0]);
        Swap(0, Keys.Count - 1);
        Keys.RemoveAt(Keys.Count - 1);
        Values.RemoveAt(Values.Count - 1);
        HeapIndices.Remove(result.Value);

        BubbleDown(0);

        HeapIndices.Remove(result.Value);
        return result;
    }
    public void UpdateKey(TValue value, TKey newKey)
    {
        int i;
        if (!HeapIndices.TryGetValue(value, out i))
            throw new ArgumentException("Value was not found in the heap.");

        TKey oldKey = Keys[i];
        int comp = oldKey.CompareTo(newKey);

        Keys[i] = newKey;

        if (comp < 0)
            BubbleUp(i);
        if (comp > 0)
            BubbleDown(i);
    }

    public int Count { get { return Keys.Count; } }

    public bool IsEmpty { get { return Count == 0; } }

    public void Clear()
    {
        Values.Clear();
        Keys.Clear();
        HeapIndices.Clear();
    }

    private void BubbleUp(int i)
    {
        while (i > 0 && Keys[i].CompareTo(Keys[parent(i)]) < 0)
        {
            Swap(i, parent(i));
            i = parent(i);
        }
    }
    private void BubbleDown(int i)
    {
        while (true)
        {
            int min = i;

            if (left(i) < Keys.Count && Keys[min].CompareTo(Keys[left(i)]) > 0)
                min = left(i);
            if (right(i) < Keys.Count && Keys[min].CompareTo(Keys[right(i)]) > 0)
                min = right(i);
            if (min == i)
                return;
            Swap(i, min);
            i = min;
        }
    }

    private int parent(int i) { return (i - 1) / 2; }
    private int left(int i)  { return i * 2 + 1; }
    private int right(int i) { return i * 2 + 2; }
    private void Swap(int i, int j)
    {
        TKey tmpKey = Keys[i];
        TValue tmpValue = Values[i];

        HeapIndices[Values[i]] = j;
        HeapIndices[Values[j]] = i;

        Keys[i] = Keys[j];
        Keys[j] = tmpKey;

        Values[i] = Values[j];
        Values[j] = tmpValue;
    }

}
